#include <Eigen/Dense>
#include <cmath>
#include <opencv2/opencv.hpp>
#include <string>
#include <sstream> // 用于拼接状态描述

#include "cv_bridge/cv_bridge.h"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "sensor_msgs/point_cloud2_iterator.hpp"

// 【正确】包含Service自动生成的头文件（匹配SensorStatus.srv）
#include "sensor_fusion/srv/sensor_status.hpp"

// 相机内参（原有代码不变）
const double fx = 525.0;
const double fy = 525.0;
const double cx = 319.5;
const double cy = 239.5;

// 时间同步阈值（原有代码不变）
const double TIME_SYNC_THRESHOLD = 0.1;

// 传感器活跃判断阈值：5秒内无数据则视为不活跃
const double SENSOR_INACTIVE_THRESHOLD = 5.0;

class SensorFusionNode : public rclcpp::Node
{
public:
  SensorFusionNode() : Node("sensor_fusion_node")
  {
    // 1. 订阅激光雷达点云（原有代码不变）
    lidar_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
        "lidar/points", 10,
        std::bind(&SensorFusionNode::lidar_callback, this, std::placeholders::_1));

    // 2. 订阅相机图像（原有代码不变）
    camera_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
        "camera/image_raw", 10,
        std::bind(&SensorFusionNode::camera_callback, this, std::placeholders::_1));

    // 3. 创建融合结果发布者（原有代码不变）
    fused_pub_ = this->create_publisher<sensor_msgs::msg::Image>("fused/image_with_points", 10);

    // 4. 初始化激光→相机的变换矩阵（原有代码不变）
    lidar_to_camera = Eigen::Matrix4f::Identity();
    lidar_to_camera(2, 3) = 0.5;

    // 5. 创建SensorStatus Service（服务名：/fusion/get_sensor_status）
    sensor_status_srv_ = this->create_service<sensor_fusion::srv::SensorStatus>(
        "/fusion/get_sensor_status", // 服务名（客户端调用时需匹配）
        std::bind(&SensorFusionNode::handle_sensor_status, this,
                  std::placeholders::_1, std::placeholders::_2));

    // 【新增】初始化传感器状态变量
    latest_fusion_time_ = this->get_clock()->now();
    fusion_count_ = 0;
    last_projected_points_ = 0;
    is_camera_active_ = false;
    is_lidar_active_ = false;

    RCLCPP_INFO(this->get_logger(), "Sensor fusion node initialized! (with sensor status Service)");
  }

private:
  // ------------------------------
  // 【修正】Service相关成员变量（匹配srv定义）
  // ------------------------------
  rclcpp::Service<sensor_fusion::srv::SensorStatus>::SharedPtr sensor_status_srv_; // Service对象
  rclcpp::Time latest_fusion_time_;                                                // 最近融合时间（用于状态描述）
  size_t fusion_count_;                                                            // 融合总次数（详细信息）
  size_t last_projected_points_;                                                   // 上次投影点数量（详细信息）
  bool is_camera_active_;                                                          // 相机是否活跃
  bool is_lidar_active_;                                                           // 激光雷达是否活跃

  // ------------------------------
  // 原有成员变量（保持不变）
  // ------------------------------
  sensor_msgs::msg::PointCloud2::SharedPtr latest_lidar_cloud_;
  rclcpp::Time latest_lidar_stamp_; // 激光最新数据时间戳
  sensor_msgs::msg::Image::SharedPtr latest_camera_image_;
  rclcpp::Time latest_camera_stamp_; // 相机最新数据时间戳
  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr lidar_sub_;
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr camera_sub_;
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr fused_pub_;
  Eigen::Matrix4f lidar_to_camera;

  // ------------------------------
  // 原有回调函数（更新时间戳，保持不变）
  // ------------------------------
  void lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
  {
    latest_lidar_cloud_ = msg;
    latest_lidar_stamp_ = msg->header.stamp;
    if (latest_camera_image_)
    {
      try_fuse();
    }
  }

  void camera_callback(const sensor_msgs::msg::Image::SharedPtr msg)
  {
    latest_camera_image_ = msg;
    latest_camera_stamp_ = msg->header.stamp;
    if (latest_lidar_cloud_)
    {
      try_fuse();
    }
  }

  // ------------------------------
  // 原有融合逻辑（新增传感器活跃状态更新）
  // ------------------------------
  void try_fuse()
  {
    double time_diff = std::abs(latest_lidar_stamp_.seconds() -
                                latest_camera_stamp_.seconds() +
                                (latest_lidar_stamp_.nanoseconds() -
                                 latest_camera_stamp_.nanoseconds()) /
                                    1e9);

    if (time_diff > TIME_SYNC_THRESHOLD)
    {
      RCLCPP_DEBUG(this->get_logger(),
                   "Time sync failed! Diff: %.3fs (threshold: %.3fs)",
                   time_diff, TIME_SYNC_THRESHOLD);
      // 即使同步失败，也更新传感器活跃状态（只要有数据就视为活跃）
      update_sensor_active_status();
      return;
    }

    // 融合成功：更新时间、计数、活跃状态
    latest_fusion_time_ = this->get_clock()->now();
    fusion_count_++;
    is_camera_active_ = true;
    is_lidar_active_ = true;

    do_fusion();
  }

  void do_fusion()
  {
    try
    {
      // 原有融合逻辑（图像转换、点云投影）完全不变
      cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(
          latest_camera_image_, sensor_msgs::image_encodings::BGR8);
      cv::Mat fused_image = cv_ptr->image;

      sensor_msgs::PointCloud2Iterator<float> it_x(*latest_lidar_cloud_, "x");
      sensor_msgs::PointCloud2Iterator<float> it_y(*latest_lidar_cloud_, "y");
      sensor_msgs::PointCloud2Iterator<float> it_z(*latest_lidar_cloud_, "z");

      size_t projected_points = 0;
      for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z)
      {
        float lidar_x = *it_x;
        float lidar_y = *it_y;
        float lidar_z = *it_z;

        if (lidar_z <= 0 || lidar_z > 10.0)
        {
          continue;
        }

        Eigen::Vector4f lidar_point(lidar_x, lidar_y, lidar_z, 1.0f);
        Eigen::Vector4f camera_point = lidar_to_camera * lidar_point;

        float pixel_u = fx * (camera_point.x() / camera_point.z()) + cx;
        float pixel_v = fy * (camera_point.y() / camera_point.z()) + cy;

        if (pixel_u < 0 || pixel_u >= fused_image.cols || pixel_v < 0 ||
            pixel_v >= fused_image.rows)
        {
          continue;
        }

        cv::Scalar point_color;
        if (lidar_z < 3.0)
        {
          point_color = cv::Scalar(0, 0, 255);
        }
        else if (lidar_z < 7.0)
        {
          point_color = cv::Scalar(0, 255, 0);
        }
        else
        {
          point_color = cv::Scalar(255, 0, 0);
        }

        cv::circle(fused_image, cv::Point(static_cast<int>(pixel_u), static_cast<int>(pixel_v)),
                   2, point_color, -1);
        projected_points++;
      }

      // 记录上次投影点数量（用于状态描述）
      last_projected_points_ = projected_points;

      // 绘制融合说明文字
      cv::putText(fused_image, "Fused: Camera + LiDAR (Sync: <100ms)",
                  cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 0.8,
                  cv::Scalar(0, 255, 255), 2);

      // 发布融合结果
      cv_bridge::CvImage ros_fused_msg;
      ros_fused_msg.header = latest_camera_image_->header;
      ros_fused_msg.encoding = sensor_msgs::image_encodings::BGR8;
      ros_fused_msg.image = fused_image;
      fused_pub_->publish(*ros_fused_msg.toImageMsg());
    }
    catch (cv_bridge::Exception &e)
    {
      RCLCPP_ERROR(this->get_logger(), "cv_bridge error: %s", e.what());
    }
    catch (std::exception &e)
    {
      RCLCPP_ERROR(this->get_logger(), "Fusion error: %s", e.what());
    }
  }

  // ------------------------------
  // 【新增】辅助函数：更新传感器活跃状态（5秒无数据则不活跃）
  // ------------------------------
  void update_sensor_active_status()
  {
    double now = this->get_clock()->now().seconds();
    // 相机活跃判断：有数据且时间戳在5秒内
    is_camera_active_ = (latest_camera_image_ != nullptr) &&
                        (now - latest_camera_stamp_.seconds() < SENSOR_INACTIVE_THRESHOLD);
    // 激光雷达活跃判断：有数据且时间戳在5秒内
    is_lidar_active_ = (latest_lidar_cloud_ != nullptr) &&
                       (now - latest_lidar_stamp_.seconds() < SENSOR_INACTIVE_THRESHOLD);
  }

  void handle_sensor_status(
      const std::shared_ptr<sensor_fusion::srv::SensorStatus::Request> req,
      const std::shared_ptr<sensor_fusion::srv::SensorStatus::Response> res)
  {
    // 1. 先更新传感器活跃状态（确保判断最新）
    update_sensor_active_status();

    // 2. 解析请求的sensor_id（0=所有，1=相机，2=激光雷达）
    int32_t target_sensor = req->sensor_id;
    RCLCPP_INFO(this->get_logger(), "Received sensor status request: sensor_id = %d", target_sensor);

    // 3. 初始化响应默认值
    res->camera_active = false;
    res->lidar_active = false;
    res->return_code = 0;        // 0=成功（默认）
    std::stringstream status_ss; // 拼接状态描述

    // 4. 根据sensor_id填充响应（核心逻辑）
    switch (target_sensor)
    {
    case 0: // 查询所有传感器
      res->camera_active = is_camera_active_;
      res->lidar_active = is_lidar_active_;
      // 拼接状态描述（包含融合统计信息）
      status_ss << "All sensors status:\n"
                << "  - Camera: " << (is_camera_active_ ? "Active" : "Inactive") << "\n"
                << "  - LiDAR: " << (is_lidar_active_ ? "Active" : "Inactive") << "\n"
                << "Fusion stats:\n"
                << "  - Total fusion count: " << fusion_count_ << "\n"
                << "  - Last projected points: " << last_projected_points_ << "\n"
                << "  - Last fusion time: " << std::fixed << std::setprecision(3)
                << latest_fusion_time_.seconds() << "s";
      // 错误码判断（有传感器不活跃则设对应错误码）
      if (!is_camera_active_ && !is_lidar_active_)
      {
        res->return_code = 3; // 3=所有传感器不活跃
        status_ss << "\nError: All sensors inactive (no data in " << SENSOR_INACTIVE_THRESHOLD << "s)";
      }
      else if (!is_camera_active_)
      {
        res->return_code = 1; // 1=相机不活跃
        status_ss << "\nError: Camera inactive (no data in " << SENSOR_INACTIVE_THRESHOLD << "s)";
      }
      else if (!is_lidar_active_)
      {
        res->return_code = 2; // 2=激光雷达不活跃
        status_ss << "\nError: LiDAR inactive (no data in " << SENSOR_INACTIVE_THRESHOLD << "s)";
      }
      break;

    case 1: // 仅查询相机
      res->camera_active = is_camera_active_;
      res->lidar_active = false; // 仅查相机，激光字段设为false
      status_ss << "Camera status: " << (is_camera_active_ ? "Active" : "Inactive");
      if (!is_camera_active_)
      {
        res->return_code = 1;
        status_ss << " (Error: No data in " << SENSOR_INACTIVE_THRESHOLD << "s)";
      }
      break;

    case 2:                       // 仅查询激光雷达
      res->camera_active = false; // 仅查激光，相机字段设为false
      res->lidar_active = is_lidar_active_;
      status_ss << "LiDAR status: " << (is_lidar_active_ ? "Active" : "Inactive");
      if (!is_lidar_active_)
      {
        res->return_code = 2;
        status_ss << " (Error: No data in " << SENSOR_INACTIVE_THRESHOLD << "s)";
      }
      break;

    default:                // 无效sensor_id
      res->return_code = 4; // 4=无效ID
      status_ss << "Error: Invalid sensor_id! Use 0 (all), 1 (camera), 2 (lidar)";
      break;
    }

    // 5. 填充响应的状态描述
    res->status_message = status_ss.str();

    // 6. 日志打印响应结果（方便调试）
    RCLCPP_INFO(this->get_logger(), "Response to sensor_id=%d:\n"
                                    "  camera_active: %s\n"
                                    "  lidar_active: %s\n"
                                    "  return_code: %d\n"
                                    "  status_message: %s",
                target_sensor,
                res->camera_active ? "true" : "false",
                res->lidar_active ? "true" : "false",
                res->return_code,
                res->status_message.c_str());
  }
};

// 主函数（原有代码不变）
int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  auto fusion_node = std::make_shared<SensorFusionNode>();
  rclcpp::spin(fusion_node);
  rclcpp::shutdown();
  return 0;
}